
// Created by gaoxiang on 19-5-4.
//

#include <gflags/gflags.h>
#include "myslam/pic_sample.h"
#include "myslam/stereo_undistort.h"
#include "myslam/visual_odometry.h"
#include "myslam/transfer.h"






DEFINE_string(config_file, "./config/default.yaml", "config file path");

const uint8_t  start_buf[3] = {0xfe,0xfd,0xfc};
const uint8_t  stop_buf[3] = {0xfc,0xfd,0xfe};

int main(int argc, char **argv) {
    FLAGS_log_dir = "./log"; 
    google::ParseCommandLineFlags(&argc, &argv, true);
    google::InitGoogleLogging(argv[0]);
    myslam::VisualOdometry::Ptr vo(new myslam::VisualOdometry(FLAGS_config_file));

    myslam::Transfer::Ptr transfer_ = myslam::Transfer::Ptr(new myslam::Transfer);
    myslam::Pic_Sample::Ptr pic_sample_ = myslam::Pic_Sample::Ptr(new myslam::Pic_Sample);
    myslam::Stereo_Undistort::Ptr stereo_undistort_ = myslam::Stereo_Undistort::Ptr(new myslam::Stereo_Undistort);
    pic_sample_->SetStereoUndistort(stereo_undistort_);
    stereo_undistort_->SetVisualOdometry(vo);
    vo->SetTransfer(transfer_);
    
     if(transfer_->fd<0)
        exit(0);
    else{
        int n = write(transfer_->fd,start_buf,3);
        printf("start : n = %d\n\r",n);

        sleep(1);
        n = write(transfer_->fd,start_buf,3);
        printf("start : n = %d\n\r",n);

        sleep(1);
        n = write(transfer_->fd,start_buf,3);
        printf("start : n = %d\n\r",n);

    }

    while(1){
        sleep(1);
    }
    return 0;
}



